#include "app_ICM42688_drv.h"
#include "tasks.h"
#if defined(ICM_USE_HARD_SPI)
#elif defined(ICM_USE_HARD_I2C)
#include "bsp_cpu_i2c2.h"
#endif
#include "stdio.h"

//SPI_HandleTypeDef *ICM42688_SPI;
IMU_Data_t ICM42688;


///*******************************************************************************
//* ��    �ƣ� bsp_IcmSpixCsInit
//* ��    �ܣ� Icm SPI��CS�������ų�ʼ��
//* ��ڲ����� ��
//* ���ڲ����� ��
//* �������ߣ� Baxiange
//* �������ڣ� 2022-07-25
//* ��    �ģ�
//* �޸����ڣ�
//* ��    ע��
//*******************************************************************************/
////void bsp_IcmSpixCsInit(void)
////{
////    GPIO_InitTypeDef GPIO_InitStruct = {0};

////    /* GPIO Ports Clock Enable */
////    ICM_RCC_SPIX_CS();

////    /*Configure GPIO pins */
////    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
////    GPIO_InitStruct.Pull = GPIO_NOPULL;
////    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
////    GPIO_InitStruct.Pin = ICM_PIN_SPIX_CS;
////    HAL_GPIO_Init(ICM_PORT_SPIX_CS, &GPIO_InitStruct);
////    HAL_GPIO_WritePin(ICM_PORT_SPIX_CS, ICM_PIN_SPIX_CS, GPIO_PIN_SET);
////}

///*******************************************************************************
//* ��    �ƣ� Icm_Spi_ReadWriteNbytes
//* ��    �ܣ� ʹ��SPI��дn���ֽ�
//* ��ڲ����� pBuffer: д�������  len:д������ĳ���
//* ���ڲ����� ��
//* �������ߣ� Baxiange
//* �������ڣ� 2022-07-25
//* ��    �ģ�
//* �޸����ڣ�
//* ��    ע��
//*******************************************************************************/
//static void Icm_Spi_ReadWriteNbytes(uint8_t* pBuffer, uint8_t len)
//{
//  HAL_SPI_TransmitReceive(&ICM42688_infor,pBuffer,pBuffer,len,1000);
//}
//#elif defined(ICM_USE_HARD_I2C)
////iic�ӿ�
//#endif

///*******************************************************************************
//* ��    �ƣ� icm42688_read_reg
//* ��    �ܣ� ��ȡ�����Ĵ�����ֵ
//* ��ڲ����� reg: �Ĵ�����ַ
//* ���ڲ����� ��ǰ�Ĵ�����ַ��ֵ
//* �������ߣ� Baxiange
//* �������ڣ� 2022-07-25
//* ��    �ģ�
//* �޸����ڣ�
//* ��    ע�� ʹ��SPI��ȡ�Ĵ���ʱҪע��:���λΪ��дλ�����datasheet page51.
//*******************************************************************************/
//static uint8_t icm42688_read_reg(uint8_t reg)
//{
//    uint8_t regval[2];

//#if defined(ICM_USE_HARD_SPI)
//    ICM_SPI_CS_LOW();
////		printf("ICM_SPI_CS_LOW\n");
//    regval[0]= reg |0x80;
//    /* д��Ҫ���ļĴ�����ַ */
//    Icm_Spi_ReadWriteNbytes(regval, 2);
////    /* ��ȡ�Ĵ������� */
////    Icm_Spi_ReadWriteNbytes(&regval, 1);
//    ICM_SPI_CS_HIGH();
////		printf("ICM_SPI_CS_HIGH\n");
//#elif defined(ICM_USE_HARD_I2C)

//#endif

//    return regval[1];
//}

///*******************************************************************************
//* ��    �ƣ� icm42688_read_regs
//* ��    �ܣ� ������ȡ����Ĵ�����ֵ
//* ��ڲ����� reg: ��ʼ�Ĵ�����ַ *buf����ָ��,uint16_t len����
//* ���ڲ����� ��
//* �������ߣ� Baxiange
//* �������ڣ� 2022-07-25
//* ��    �ģ�
//* �޸����ڣ�
//* ��    ע�� ʹ��SPI��ȡ�Ĵ���ʱҪע��:���λΪ��дλ�����datasheet page50.
//*******************************************************************************/
//static void icm42688_read_regs(uint8_t reg, uint8_t* buf, uint16_t len)
//{
//  uint8_t Buffer[len+1];
//#if defined(ICM_USE_HARD_SPI)
//  Buffer[0]=reg|0x80;
//    ICM_SPI_CS_LOW();
//    /* д��Ҫ���ļĴ�����ַ */
//    Icm_Spi_ReadWriteNbytes(Buffer, 1+len);
//    /* ��ȡ�Ĵ������� */
//  for(uint16_t i=0;i<len;i++)
//  {
//    *(buf+i)=Buffer[i+1];
//  }
//    ICM_SPI_CS_HIGH();
//#elif defined(ICM_USE_HARD_I2C)
//  
//#endif
//}


///*******************************************************************************
//* ��    �ƣ� icm42688_write_reg
//* ��    �ܣ� �򵥸��Ĵ���д����
//* ��ڲ����� reg: �Ĵ�����ַ value:����
//* ���ڲ����� 0
//* �������ߣ� Baxiange
//* �������ڣ� 2022-07-25
//* ��    �ģ�
//* �޸����ڣ�
//* ��    ע�� ʹ��SPI��ȡ�Ĵ���ʱҪע��:���λΪ��дλ�����datasheet page50.
//*******************************************************************************/
//static uint8_t icm42688_write_reg(uint8_t reg, uint8_t value)
//{
//#if defined(ICM_USE_HARD_SPI)
//  uint8_t regval[2];
//  /* д��Ҫ���ļĴ�����ַ */
//  regval[0]=reg;
///* ��ȡ�Ĵ������� */
//  regval[1]=value
//  ICM_SPI_CS_LOW();
//  Icm_Spi_ReadWriteNbytes(regval,2);
//  ICM_SPI_CS_HIGH();
//#elif defined(ICM_USE_HARD_I2C)
//  
//#endif
//    return 0;
//}



//float bsp_Icm42688GetAres(uint8_t Ascale)
//{
//    switch(Ascale)
//    {
//    // Possible accelerometer scales (and their register bit settings) are:
//    // 2 Gs (11), 4 Gs (10), 8 Gs (01), and 16 Gs  (00).
//		case AFS_2G:
//        accSensitivity = 10 / 16384.0f;
//        break;
//    case AFS_4G:
//        accSensitivity = 10 / 8192.0f;
//        break;
//    case AFS_8G:
//        accSensitivity = 10 / 4096.0f;
//        break;
//    case AFS_16G:
//        accSensitivity = 10 / 2048.0f;
//        break;
////    case AFS_2G:
////        accSensitivity = 20 / 32768.0f;
////        break;
////    case AFS_4G:
////        accSensitivity = 40 / 32768.0f;
////        break;
////    case AFS_8G:
////        accSensitivity = 80 / 32768.0f;
////        break;
////    case AFS_16G:
////        accSensitivity = 160 / 32768.0f;
////        break;
//    }

//    return accSensitivity;
//}

//float bsp_Icm42688GetGres(uint8_t Gscale)
//{
//    switch(Gscale)
//		{
//    case GFS_15_125DPS:
//        gyroSensitivity = PI/180/2097.2;
//        break;
//    case GFS_31_25DPS:
//        gyroSensitivity = PI/180/1048.6;
//        break;
//    case GFS_62_5DPS:
//				gyroSensitivity = PI/180/524.3;
//        break;
//    case GFS_125DPS:
//        gyroSensitivity = PI/180/262;
//        break;
//    case GFS_250DPS:
//		    gyroSensitivity = PI/180/131;
//        break;
//    case GFS_500DPS:
//        gyroSensitivity = PI/180/65.5;
//        break;
//    case GFS_1000DPS:
//        gyroSensitivity = PI/180/32.8;
//        break;
//    case GFS_2000DPS:
//			  gyroSensitivity = PI/180/16.4;
//        break;
//    }
////    {
////    case GFS_15_125DPS:
////        gyroSensitivity = 15.625f / 32768.0;
////        break;
////    case GFS_31_25DPS:
////        gyroSensitivity = 31.25f / 32768.0;
////        break;
////    case GFS_62_5DPS:
////        gyroSensitivity = 62.5f / 32768.0;
////        break;
////    case GFS_125DPS:
////        gyroSensitivity = 125.0f / 32768.0;
////        break;
////    case GFS_250DPS:
////        gyroSensitivity = 250.0f / 32768.0;
////        break;
////    case GFS_500DPS:
////        gyroSensitivity = 500.0f / 32768.0;
////        break;
////    case GFS_1000DPS:
////        gyroSensitivity = 1000.0f / 32768.0;
////        break;
////    case GFS_2000DPS:
////        gyroSensitivity = 2000.0f / 32768.0f;
////        break;
////    }
//    return gyroSensitivity;
//}

///*******************************************************************************
//* ��    �ƣ� bsp_Icm42688RegCfg
//* ��    �ܣ� Icm42688 �Ĵ�������
//* ��ڲ����� ��
//* ���ڲ����� ��
//* �������ߣ� Baxiange
//* �������ڣ� 2022-07-25
//* ��    �ģ�
//* �޸����ڣ�
//* ��    ע��
//*******************************************************************************/
//int8_t bsp_Icm42688RegCfg(void)
//{
//    uint8_t reg_val = 0;
//    /* ��ȡ who am i �Ĵ��� */
//    reg_val = icm42688_read_reg(ICM42688_WHO_AM_I);
////		printf("reg_val:%d\n",reg_val);
//    icm42688_write_reg(ICM42688_REG_BANK_SEL, 0); //����bank 0����Ĵ���
//    icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x01); //����λ������
//    ICM42688DelayMs(100);


//    if(reg_val == ICM42688_ID)
//    {
//        icm42688_write_reg(ICM42688_REG_BANK_SEL, 1); //����bank 1����Ĵ���
//        icm42688_write_reg(ICM42688_INTF_CONFIG4, 0x02); //����Ϊ4��SPIͨ��

//        icm42688_write_reg(ICM42688_REG_BANK_SEL, 0); //����bank 0����Ĵ���
//        icm42688_write_reg(ICM42688_FIFO_CONFIG, 0x40); //Stream-to-FIFO Mode(page63)


//        reg_val = icm42688_read_reg(ICM42688_INT_SOURCE0);
//        icm42688_write_reg(ICM42688_INT_SOURCE0, 0x00);
//        icm42688_write_reg(ICM42688_FIFO_CONFIG2, 0x00); // watermark
//        icm42688_write_reg(ICM42688_FIFO_CONFIG3, 0x02); // watermark
//        icm42688_write_reg(ICM42688_INT_SOURCE0, reg_val);
//        icm42688_write_reg(ICM42688_FIFO_CONFIG1, 0x63); // Enable the accel and gyro to the FIFO

//        icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x00);
//        reg_val = icm42688_read_reg(ICM42688_INT_CONFIG);
//        reg_val |= (1 << 1);
//        reg_val |= 1 ;
//        icm42688_write_reg(ICM42688_INT_CONFIG, reg_val);//�����ж�1Ϊ����ģʽ������������͵�ƽ��Ч
//      
//        icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x00);
//        reg_val = icm42688_read_reg(ICM42688_INT_SOURCE0);
//        reg_val |= (1 << 3); //
//        icm42688_write_reg(ICM42688_INT_SOURCE0, reg_val);
//      
//        icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x00);
//        reg_val = icm42688_read_reg(ICM42688_INT_CONFIG1);
//        reg_val &= 0XEF;
//        icm42688_write_reg(ICM42688_INT_CONFIG1, reg_val);//

//        bsp_Icm42688GetAres(AFS_8G);
//        icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x00);
//        reg_val = icm42688_read_reg(ICM42688_ACCEL_CONFIG0);//page74
//        reg_val |= (AFS_8G << 5);   //���� ��8g
//        reg_val |= (AODR_50Hz);     //������� 50HZ
//        icm42688_write_reg(ICM42688_ACCEL_CONFIG0, reg_val);

//        bsp_Icm42688GetGres(GFS_1000DPS);
//        icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x00);
//        reg_val = icm42688_read_reg(ICM42688_GYRO_CONFIG0);//page73
//        reg_val |= (GFS_1000DPS << 5);   //���� ��1000dps
//        reg_val |= (GODR_50Hz);     //������� 50HZ
//        icm42688_write_reg(ICM42688_GYRO_CONFIG0, reg_val);

//        icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x00);
//        reg_val = icm42688_read_reg(ICM42688_PWR_MGMT0); //��ȡPWR��MGMT0��ǰ�Ĵ�����ֵ(page72)
//        reg_val &= ~(1 << 5);//ʹ���¶Ȳ���
//        reg_val |= ((3) << 2);//����GYRO_MODE  0:�ر� 1:���� 2:Ԥ�� 3:������
//        reg_val |= (3);//����ACCEL_MODE 0:�ر� 1:�ر� 2:�͹��� 3:������
//        icm42688_write_reg(ICM42688_PWR_MGMT0, reg_val);
//        ICM42688DelayMs(1); //������PWR��MGMT0�Ĵ����� 200us�ڲ������κζ�д�Ĵ����Ĳ���

//        return 0;
//    }
//    return -1;
//}
///*******************************************************************************
//* ��    �ƣ� bsp_Icm42688Init
//* ��    �ܣ� Icm42688 ��������ʼ��
//* ��ڲ����� ��
//* ���ڲ����� 0: ��ʼ���ɹ�  ����ֵ: ��ʼ��ʧ��
//* �������ߣ� Baxiange
//* �������ڣ� 2022-07-25
//* ��    �ģ�
//* �޸����ڣ�
//* ��    ע��
//*******************************************************************************/
//int8_t bsp_Icm42688Init(void)
//{
////    bsp_IcmSpixCsInit();
////		 SPI_Init(); //hal���Ѿ�Ӳ����ʼ����

//    return(bsp_Icm42688RegCfg());

//}

///*******************************************************************************
//* ��    �ƣ� bsp_IcmGetTemperature
//* ��    �ܣ� ��ȡIcm42688 �ڲ��������¶�
//* ��ڲ����� ��
//* ���ڲ����� ��
//* �������ߣ� Baxiange
//* �������ڣ� 2022-07-25
//* ��    �ģ�
//* �޸����ڣ�
//* ��    ע�� datasheet page62
//*******************************************************************************/
//int8_t bsp_IcmGetTemperature(int16_t* pTemp)
//{
//    uint8_t buffer[2] = {0};

//    icm42688_read_regs(ICM42688_TEMP_DATA1, buffer, 2);

//    *pTemp = (int16_t)(((int16_t)((buffer[0] << 8) | buffer[1])) / 132.48 + 25);
//    return 0;
//}

///*******************************************************************************
//* ��    �ƣ� bsp_IcmGetAccelerometer
//* ��    �ܣ� ��ȡIcm42688 ���ٶȵ�ֵ
//* ��ڲ����� ������ٶȵ�ֵ
//* ���ڲ����� ��
//* �������ߣ� Baxiange
//* �������ڣ� 2022-07-25
//* ��    �ģ�
//* �޸����ڣ�
//* ��    ע�� datasheet page62
//*******************************************************************************/
//int8_t bsp_IcmGetAccelerometer(icm42688RawData_t* accData)
//{
//    uint8_t buffer[6] = {0};

//    icm42688_read_regs(ICM42688_ACCEL_DATA_X1, buffer, 6);

//    accData->x = ((uint16_t)buffer[0] << 8) | buffer[1];
//    accData->y = ((uint16_t)buffer[2] << 8) | buffer[3];
//    accData->z = ((uint16_t)buffer[4] << 8) | buffer[5];

//    accData->x = (int16_t)(accData->x * accSensitivity);
//    accData->y = (int16_t)(accData->y * accSensitivity);
//    accData->z = (int16_t)(accData->z * accSensitivity);

//    return 0;
//}

///*******************************************************************************
//* ��    �ƣ� bsp_IcmGetGyroscope
//* ��    �ܣ� ��ȡIcm42688 �����ǵ�ֵ
//* ��ڲ����� ���������ǵ�ֵ
//* ���ڲ����� ��
//* �������ߣ� Baxiange
//* �������ڣ� 2022-07-25
//* ��    �ģ�
//* �޸����ڣ�
//* ��    ע�� datasheet page63
//*******************************************************************************/
//int8_t bsp_IcmGetGyroscope(icm42688RawData_t* GyroData)
//{
//    uint8_t buffer[6] = {0};

//    icm42688_read_regs(ICM42688_GYRO_DATA_X1, buffer, 6);

//    GyroData->x = ((uint16_t)buffer[0] << 8) | buffer[1];
//    GyroData->y = ((uint16_t)buffer[2] << 8) | buffer[3];
//    GyroData->z = ((uint16_t)buffer[4] << 8) | buffer[5];

//    GyroData->x = (int16_t)(GyroData->x * gyroSensitivity);
//    GyroData->y = (int16_t)(GyroData->y * gyroSensitivity);
//    GyroData->z = (int16_t)(GyroData->z * gyroSensitivity);
//    return 0;
//}

///*******************************************************************************
//* ��    �ƣ� bsp_IcmGetRawData
//* ��    �ܣ� ��ȡIcm42688���ٶ�����������
//* ��ڲ����� ����
//* ���ڲ����� ��
//* �������ߣ� Baxiange
//* �������ڣ� 2022-07-25
//* ��    �ģ�
//* �޸����ڣ�
//* ��    ע�� datasheet page62,63
//*******************************************************************************/
//uint8_t INT_STATUS_valu,INT_STATUS_valu1,INT_STATUS_valu2,INT_SOURCE0_valu,INT_CONFIG0_valu;
//int8_t bsp_IcmGetRawData(IMU_Data_t* ICM42688_temp)
//{
//    uint8_t buffer[12] = {0};
//		static int16_t icm42688_raw_temp;
////    INT_STATUS_valu = icm42688_read_reg(ICM42688_INT_STATUS);
////    INT_STATUS_valu1 = icm42688_read_reg(ICM42688_INT_STATUS);
//    icm42688_read_regs(ICM42688_ACCEL_DATA_X1, buffer, 12);

//    icm42688_raw_temp  = (int16_t)((buffer[0] << 8)  | buffer[1]);
//		ICM42688_temp->Accel[0]= icm42688_raw_temp * accSensitivity;
//		
//    icm42688_raw_temp  = (int16_t)((buffer[2] << 8)  | buffer[3]);
//		ICM42688_temp->Accel[1] = icm42688_raw_temp* accSensitivity;

//    icm42688_raw_temp  = (int16_t)((buffer[4] << 8)  | buffer[5]);
//		ICM42688_temp->Accel[2] = icm42688_raw_temp* accSensitivity;

//    icm42688_raw_temp = (int16_t)((buffer[6] << 8)  | buffer[7]);
//		ICM42688_temp->Gyro[0] = icm42688_raw_temp* gyroSensitivity;
//		
//    icm42688_raw_temp = (int16_t)((buffer[8] << 8)  | buffer[9]);
//		ICM42688_temp->Gyro[1]= icm42688_raw_temp * gyroSensitivity;

//    icm42688_raw_temp = (int16_t)((buffer[10] << 8) | buffer[11]);
//    ICM42688_temp->Gyro[2] = icm42688_raw_temp * gyroSensitivity;
////,������Ư��ʱ��
//		if(ICM42688.num>8000)
//		{		
//			ICM42688_temp->num=0;
//      ICM42688_temp->AccelScale=0;
//		  ICM42688_temp->GyroOffset[0]=0;
//		  ICM42688_temp->GyroOffset[1]=0;
//		  ICM42688_temp->GyroOffset[2]=0;
//		}
//    ICM42688_temp->AccelScale+=sqrt(pow(ICM42688_temp->Accel[0],2)+pow(ICM42688_temp->Accel[1],2)+pow(ICM42688_temp->Accel[2],2));
//		ICM42688_temp->GyroOffset[0]+=ICM42688_temp->Gyro[0];
//		ICM42688_temp->GyroOffset[1]+=ICM42688_temp->Gyro[1];
//		ICM42688_temp->GyroOffset[2]+=ICM42688_temp->Gyro[2];
//		
//		ICM42688_temp->num++;
//		
//		ICM42688_temp->gNorm= ICM42688_temp->AccelScale/ICM42688_temp->num;
//		ICM42688_temp->GyroOffset_now[0]=ICM42688_temp->GyroOffset[0]/ICM42688_temp->num;
//		ICM42688_temp->GyroOffset_now[1]=ICM42688_temp->GyroOffset[1]/ICM42688_temp->num;
//		ICM42688_temp->GyroOffset_now[2]=ICM42688_temp->GyroOffset[2]/ICM42688_temp->num;

//    return 0;
//}

//// ��׼��Ʈ
//void Calibrate_MPU_Offset(IMU_Data_t *icm42688)
//{
//    static float startTime;
//    static uint16_t CaliTimes = 6000; // ��Ҫ�㹻������ݲ��ܵõ���Ч��������ƫУ׼���
//    uint8_t buf[12] = {0};
//    int16_t icm42688_raw_temp;
//    float gyroMax[3], gyroMin[3];
//    float gNormTemp, gNormMax, gNormMin;

//    startTime = DWT_GetTimeline_s();
//    do
//    {
//        if (DWT_GetTimeline_s() - startTime > 10)
//        {
//            // У׼��ʱ
//            icm42688->GyroOffset[0] = GxOFFSET;
//            icm42688->GyroOffset[1] = GyOFFSET;
//            icm42688->GyroOffset[2] = GzOFFSET;
//            icm42688->gNorm = gNORM;
//            icm42688->TempWhenCali = 40;
//            break;
//        }
//        DWT_Delay(0.005);
//        icm42688->gNorm = 0;
//        icm42688->GyroOffset[0] = 0;
//        icm42688->GyroOffset[1] = 0;
//        icm42688->GyroOffset[2] = 0;

//        for (uint16_t i = 0; i < CaliTimes; i++)
//        {
//					  icm42688_read_regs(ICM42688_ACCEL_DATA_X1, buf, 12);

//            icm42688_raw_temp = (int16_t)((buf[0]) << 8) | buf[1];
//            icm42688->Accel[0] = icm42688_raw_temp * accSensitivity;
//            icm42688_raw_temp = (int16_t)((buf[2]) << 8) | buf[3];
//            icm42688->Accel[1] = icm42688_raw_temp * accSensitivity;
//            icm42688_raw_temp = (int16_t)((buf[4]) << 8) | buf[5];
//            icm42688->Accel[2] = icm42688_raw_temp * accSensitivity;
//					
//            gNormTemp = sqrtf(icm42688->Accel[0] * icm42688->Accel[0] +
//                              icm42688->Accel[1] * icm42688->Accel[1] +
//                              icm42688->Accel[2] * icm42688->Accel[2]);
//            icm42688->gNorm += gNormTemp;
//					
////					  icm42688_read_regs(ICM42688_GYRO_DATA_X1, buf, 6);

//						icm42688_raw_temp = (int16_t)((buf[6]) << 8) | buf[7];
//						icm42688->Gyro[0] = icm42688_raw_temp * gyroSensitivity;
//						icm42688->GyroOffset[0] += icm42688->Gyro[0];
//					
//						icm42688_raw_temp = (int16_t)((buf[8]) << 8) | buf[9];
//						icm42688->Gyro[1] = icm42688_raw_temp * gyroSensitivity;
//						icm42688->GyroOffset[1] += icm42688->Gyro[1];
//						
//						icm42688_raw_temp = (int16_t)((buf[10]) << 8) | buf[11];
//						icm42688->Gyro[2] = icm42688_raw_temp * gyroSensitivity;
//						icm42688->GyroOffset[2] += icm42688->Gyro[2];

//            // ��¼���ݼ���
//            if (i == 0)
//            {
//                gNormMax = gNormTemp;
//                gNormMin = gNormTemp;
//                for (uint8_t j = 0; j < 3; j++)
//                {
//                    gyroMax[j] = icm42688->Gyro[j];
//                    gyroMin[j] = icm42688->Gyro[j];
//                }
//            }
//            else
//            {
//                if (gNormTemp > gNormMax)
//                    gNormMax = gNormTemp;
//                if (gNormTemp < gNormMin)
//                    gNormMin = gNormTemp;
//                for (uint8_t j = 0; j < 3; j++)
//                {
//                    if (icm42688->Gyro[j] > gyroMax[j])
//                        gyroMax[j] = icm42688->Gyro[j];
//                    if (icm42688->Gyro[j] < gyroMin[j])
//                        gyroMin[j] = icm42688->Gyro[j];
//                }
//            }

//            // ���ݲ��������Ϊ�յ������ţ�������У׼
//            gNormDiff = gNormMax - gNormMin;
//            for (uint8_t j = 0; j < 3; j++)
//                gyroDiff[j] = gyroMax[j] - gyroMin[j];
//            if (gNormDiff > 0.5f ||
//                gyroDiff[0] > 0.15f ||
//                gyroDiff[1] > 0.15f ||
//                gyroDiff[2] > 0.15f)
//                break;
//            DWT_Delay(0.0005);
//        }

//        // ȡƽ��ֵ�õ��궨���
//        icm42688->gNorm /=CaliTimes;
//        for (uint8_t i = 0; i < 3; i++)
//            icm42688->GyroOffset_now[i] =icm42688->GyroOffset[i]/CaliTimes;

//        // ��¼�궨ʱIMU�¶�
//				    icm42688_read_regs(ICM42688_TEMP_DATA1, buf, 2);
//				  icm42688_raw_temp = (int16_t)((buf[0] << 8) | (buf[1]));

//        if (icm42688_raw_temp > 1023)
//            icm42688_raw_temp -= 2048;
//        icm42688->TempWhenCali = icm42688_raw_temp * ICM42688_TEMP_FACTOR + ICM42688_TEMP_FACTOR;

//     } while (gNormDiff > 0.5f ||
//             fabsf(icm42688->gNorm - 9.8f) > 0.5f ||
//             gyroDiff[0] > 0.15f ||
//             gyroDiff[1] > 0.15f ||
//             gyroDiff[2] > 0.15f ||
//             fabsf(icm42688->GyroOffset[0]) > 0.01f ||
//             fabsf(icm42688->GyroOffset[1]) > 0.01f ||
//             fabsf(icm42688->GyroOffset[2]) > 0.01f);

//    // ���ݱ궨���У׼���ٶȼƱ������
//    icm42688->AccelScale = g_HENGYANG / icm42688->gNorm;
////    icm42688->AccelScale = 1;

//}


